#pragma ident "$Id: SolverGFIF.hpp 2648 2011-06-09 08:01:41Z shjzhang $"

/**
 * @file SolverGFIF.hpp
 * General Solver.
 */

#ifndef GPSTK_SolverGFIF_HPP
#define GPSTK_SolverGFIF_HPP

//============================================================================
//
//  Revision
//
//  2019/08/13  
//  solve the ifcb using ppp method
//  shjzhang    
//
//============================================================================


#include "SolverBase.hpp"
#include "TypeID.hpp"
#include "ProcessingClass.hpp"
#include "EquationSystem.hpp"
#include "StochasticModel.hpp"
#include "SimpleKalmanFilter.hpp"


namespace gpstk
{

      /** @addtogroup GPSsolutions */
      /// @ingroup math

      //@{

      // type definition for usage in other program
   typedef std::map<CommonTime, VariableDataMap > epochVarDataMap;
   typedef std::map<CommonTime, CovarianceDataMap > epochCovDataMap;
   typedef std::map<CommonTime, sourceTypeValueMap> epochSrcDataMap;
   typedef std::map<Variable, int> VariableIntMap;


   class SolverGFIF : public SolverBase
   {
   public:

      SolverGFIF( )
          : firstTime(true), firstSolution(true),
            postMode(false), fwdProcessing(true), printSol(false)
      {
         Init();
      }

      virtual sourceTypeValueMap& Process(CommonTime& epoch,
                                          const SatID& sat, 
                                          sourceTypeValueMap& srcTypeValueData)
      {
          preCompute(epoch, sat, srcTypeValueData);
          Compute(epoch, sat, srcTypeValueData);
          postCompute(epoch, sat, srcTypeValueData);
          return srcTypeValueData;
      };

         /** Code to be executed before 'Compute()' method.
          *
          * @param gData    Data object holding the data.
          */
      virtual sourceTypeValueMap& preCompute(CommonTime& epoch,
                                             const SatID& sat, 
                                             sourceTypeValueMap& srcTypeValueData )
         noexcept(false);


         /** Code to be executed after 'Compute()' method.
          *
          * @param gData    Data object holding the data.
          */
      virtual sourceTypeValueMap& postCompute(CommonTime& epoch,
                                              const SatID& sat,
                                              sourceTypeValueMap& srcTypeValueData )
         noexcept(false);


         /** Returns a reference to a sourceTypeValueMap object after computing .
          *
          * @param gData    Data object holding the data.
          */
      virtual sourceTypeValueMap& Compute( CommonTime& epoch,
                                           const SatID& sat,
                                           sourceTypeValueMap& srcTypeValueData )
         noexcept(false);


         /** Time Update of the kalman filter.
          *
          * @param gData    Data object holding the data.
          */
      virtual int TimeUpdate( const Matrix<double>& phiMatrix,
                              const Matrix<double>& processNoiseCovariance  )
         noexcept(false);

         /** Time Update of the kalman filter.
          *
          * @param gData    Data object holding the data.
          */
      virtual int Constraint()
         noexcept(false);


         /** Measurement Update of the kalman filter.
          *
          * @param gData    Data object holding the data.
          */
      virtual int MeasUpdate( Vector<double>& prefitResiduals,
                              Matrix<double>& designMatrix,
                              Matrix<double>& weightMatrix  )
         noexcept(false);



         /// This method resets the filter, setting all variance values in
         /// covariance matrix to a very high level.
      virtual SolverGFIF& reset(void)
      { 
          firstTime = true;
          return (*this); 
      };

         /// Returns an index identifying this object.
      virtual void Init(void) ;


         /// Returns an index identifying this object.
      virtual void setUpEquations(void) ;


/*
      void setMW25File(std::string file)
      {
          obsStreamMW25.open(file);
      };

      void setGFIFFile(std::string file)
      {
          obsStreamGFIF.open(file);
      };

      void printMW25( const CommonTime& epoch, SourceIDSet sourceSet,
                      sourceTypeValueMap& srcTypeValueData)
      {
          YDSTime time( epoch );
          obsStreamMW25 << time.year << " " << time.doy  << " " << time.sod <<  " ";
          for(SourceIDSet::iterator srcIt=sourceSet.begin();
                  srcIt!=sourceSet.end();
                  srcIt++)
          {
              double mw25(0.0);
              double gfif(0.0);
              if(srcTypeValueData.find((*srcIt))!=srcTypeValueData.end()) 
              {
                  mw25 = srcTypeValueData[(*srcIt)][TypeID::MW25Frac];
                  gfif = srcTypeValueData[(*srcIt)][TypeID::GFIFFrac];
              }
              obsStreamMW25 << setprecision(6) << fixed<< setw(10) ;
              obsStreamMW25 << mw25 << " " ;
          }
          obsStreamMW25<< endl;
      };

      void printGFIF( const CommonTime& epoch, SourceIDSet sourceSet,
                      sourceTypeValueMap& srcTypeValueData)
      {
          YDSTime time( epoch );
          (obsStreamGFIF) << time.year << " " << time.doy  << " " << time.sod <<  " ";
          for(SourceIDSet::iterator srcIt=sourceSet.begin();
                  srcIt!=sourceSet.end();
                  srcIt++)
          {
              double gfif(0.0);
              if(srcTypeValueData.find((*srcIt))!=srcTypeValueData.end()) 
              {
                  gfif = srcTypeValueData[(*srcIt)][TypeID::GFIFFrac];
              }
              obsStreamGFIF << setprecision(6) << setw(10) << fixed;
              obsStreamGFIF << gfif << " ";
          }
          obsStreamGFIF<< endl;
      };
*/

      virtual double getCurrentIFCB();

         /// Returns an index identifying this object.
      virtual int getIndex(void) const;

         /// Returns a string identifying this object.
      virtual std::string getClassName(void) const;

         /// Destructor.
      virtual ~SolverGFIF() {};


   protected:

      /// Boolean indicating if this filter was run at least once
      bool firstTime;
      bool firstSolution;

      double firstIfcb;
      double currentIfcb;
      bool printSol;

      /// bwd 
      bool postMode;
      bool fwdProcessing;

      SatID currSat;

      VariableSet oldUnknowns;

         /// Boolean indicating if this filter was run at least once
      double resetInterval;

      std::ofstream fwdStream;
      std::ofstream bwdStream;
      std::ofstream bwdSmoothedStream;
      std::ofstream obsStreamMW25;
      std::ofstream obsStreamGFIF;

         /// State Transition Matrix (PhiMatrix)
      Matrix<double> phiMatrix;

         /// Noise covariance matrix (QMatrix)
      Matrix<double> qMatrix;

         /// Geometry matrix
      Matrix<double> hMatrix;

         /// Weights matrix
      Matrix<double> rMatrix;

         /// Measurements vector (Prefit-residuals)
      Vector<double> measVector;

         /// Map holding state information
      VariableDataMap stateMap;

         /// Map holding covariance information
      CovarianceDataMap covMap;

      VariableIntMap varIndexMap;

         // Predicted state
      Vector<double> xhatminus;

         // Predicted covariance.
      Matrix<double> Pminus;

         // A bwderiori state
      Vector<double> xhat;

         // A bwderiori covariance.
      Matrix<double> P;

      epochSrcDataMap forwardSrcData;

      epochVarDataMap forwardStateData;
      epochCovDataMap forwardCovData;

      epochVarDataMap allSmoothedState;
      epochCovDataMap allSmoothedCov;

   private:

      double wavelengthLC12GPS; 
      double wavelengthLC15GPS;

      double wavelengthLC15GAL;
      double wavelengthLC17GAL;
      double wavelengthLC18GAL;
      double wavelengthLC16GAL;

      double wavelengthLC27BDS;
      double wavelengthLC26BDS;

      // default model for variable define
      WhiteNoiseModel defaultModel;

      // Declare coordinate model

      // kinematic
      // xyz
      WhiteNoiseModel whiteNoiseModel;

      // GPS LC ambiguity
      PhaseAmbiguityModel ambiModel;

         /// Equation system
      EquationSystem equSystem;

         /// Initial index assigned to this class.
      static int classIndex;

         /// Index belonging to this object.
      int index;

         /// Sets the index and increment classIndex.
      void setIndex(void)
      { index = classIndex++; };


   }; // End of class 'SolverGFIF'


      //@}

}  // End of namespace gpstk
#endif   // GPSTK_SolverGFIF_HPP
